using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;
using LocalizationTest.Tools;
using System.IO;

namespace LocalizationTest
{
  public class ActionAndObservationHistory
  {
    public List<object> history; //action or observation

    public ActionAndObservationHistory()
    {
      history = new List<object>();
    }

    private List<RelativePlaceWithMeasurement> relativePlacesWithMeasurement;
    private List<WorldState> relativePlaces;
    bool relativePlacesCalculated = false;

    public List<RelativePlaceWithMeasurement> GetSelectedRelativePlaces(WorldState simulatedState) //starting with the current place
    {
      CalculateRelativePlaces(simulatedState, int.MaxValue);
      
      List<RelativePlaceWithMeasurement> toReturn = null;


      switch(Globals.selfLocatorProperties.SelectionStrategy)
      {
        case LandmarkSelectionStrategy.all:
          toReturn = relativePlacesWithMeasurement;
          break;
//        case LandmarkSelectionStrategy.contributionBased:
//          toReturn = SelectContributionBased();
//          break;
//        case LandmarkSelectionStrategy.intelligent:
//          toReturn = SelectIntelligent(simulatedState, experiment, relativePlacesWithMeasurement);
//          break;
        case LandmarkSelectionStrategy.lastAndFirstOfEachInLastN:
          toReturn = SelectLastAndFirstOfEachInLastN2(simulatedState, relativePlacesWithMeasurement);
          break;
        case LandmarkSelectionStrategy.lastN:
          toReturn = SelectLastN(simulatedState, relativePlacesWithMeasurement);
          break;
        case LandmarkSelectionStrategy.lastNOfEach:
          toReturn = SelectLastNOfEach(simulatedState, relativePlacesWithMeasurement);
          break;
        default: toReturn = relativePlacesWithMeasurement;
          break;
      }
      return toReturn;
    }

    private void CalculateRelativePlaces(WorldState simulatedState, int maxNumberOfObservations)
    {
      if (relativePlacesCalculated) return;
      relativePlacesCalculated = true;
      relativePlacesWithMeasurement = new List<RelativePlaceWithMeasurement>();
      relativePlaces = new List<WorldState>();
      WorldState currentState;

      if (simulatedState is WorldState1D) currentState = new WorldState1D(0);
      else if (simulatedState is WorldState2D) currentState = new WorldState2D(0, 0);
      else if (simulatedState is WorldStatePose2D) currentState = new WorldStatePose2D();
      else currentState = new WorldState2D(0, 0); // to be not uninitialized
      int numberOfObservations = 0;
      int indexInTimeLine = 0;
      for (int index = history.Count - 1; index >= 0; index--)
      {
        if (history[index] is Measurement && history[index] != null)
        {
          relativePlacesWithMeasurement.Add(new RelativePlaceWithMeasurement(currentState, history[index] as Measurement, index, indexInTimeLine));
          if (++numberOfObservations >= maxNumberOfObservations) break;
        }
        relativePlaces.Add(currentState);
        if (history[index] is WorldState) indexInTimeLine++;
        if (history[index] is ActionDescription)
          currentState = (history[index] as ActionDescription).ExecuteInverted(currentState);
      }
    }

    #region Add methods
    public void Add(object obj)
    {
      if (obj != null)
        history.Add(obj);
      relativePlacesCalculated = false;
    }
    public void Add(WorldState worldState)
    {
      if (worldState != null)
        history.Add(worldState);
      relativePlacesCalculated = false;
    }
    public void Add(ActionDescription action)
    {
      if (action != null)
        history.Add(action);
      relativePlacesCalculated = false;
    }
    public void Add(Measurement measurement)
    {
      if (measurement != null)
        history.Add(measurement);
      relativePlacesCalculated = false;
    }
    #endregion
    public void Delete()
    {
      history.Clear();
      relativePlacesCalculated = false;
      if (relativePlacesWithMeasurement != null) relativePlacesWithMeasurement.Clear();
      if (relativePlaces != null) relativePlaces.Clear();
    }
    /*
    public void DrawOld(WorldState simulatedState)
    {
      WorldState currentState;
      //coordinate system
      DebugDrawingTool.CreateLine("history:_coordinate system", new Vector2(-800, 0), new Vector2(800, 0), 3, Color.Gray);
      DebugDrawingTool.CreateLine("history:_coordinate system", new Vector2(0, -800), new Vector2(0, 800), 3, Color.Gray);

      DebugDrawingTool.CreateLine("time_line:_coordinate system", new Vector2(-800, 0), new Vector2(800, 0), 3, Color.Gray);
      DebugDrawingTool.CreateLine("time_line:_coordinate system", new Vector2(0, -800), new Vector2(0, 800), 3, Color.Gray);

      if (simulatedState is WorldState1D) currentState = new WorldState1D(0);
      else if (simulatedState is WorldState2D) currentState = new WorldState2D(0, 0);
      else if (simulatedState is WorldStatePose2D) currentState = new WorldStatePose2D();
      else currentState = new WorldState2D(0, 0); // to be not uninitialized

      int index = history.Count;

      double shade = 0.0;
      currentState.DrawShaded(shade, Color.Blue, "history:world states");

      int maxNumberOfObservations = int.MaxValue;
      if (DebugKeyTool.IsActive("drawing:history - limit maxNumberOfObservations")) maxNumberOfObservations = 20;
      int numberOfObservations = 0;

      int timeLineIndex = 0;
      for (index = history.Count - 1; index >= 0; index--)
      {
        if (history[index] is WorldState) timeLineIndex++;
        shade = 1.0 - (index + 1) / (double)history.Count;
        bool showColors = DebugKeyTool.IsActive("drawing:show colors in history");
        if (history[index] is ActionDescription)
        {
          currentState = (history[index] as ActionDescription).ExecuteInverted(currentState);
          //          DebugDrawingTool.CreateLine("time_line:measurements", new Vector2(index * 2, 30), new Vector2(index * 2, 35), 4, Color.Blue);
          //          DebugDrawingTool.CreateLine("time_line:measurements", new Vector2(index * 2, 30), new Vector2(index * 2, 35), 2, Color.Green);
          //          DebugDrawingTool.CreateLine("time_line:measurements", new Vector2(index * 2, 30), new Vector2(index * 2, 35), 1, Color.Red);
          DebugDrawingTool.CreateLine("time_line:measurements", new Vector2(timeLineIndex * 2, -10), new Vector2(timeLineIndex * 2, 70), 0, Color.Black);

          //          DebugDrawingTool.CreateLine("time_line:measurements", new Vector2(timeLineIndex * 2, 40), new Vector2(timeLineIndex * 2 + 2, 40), 3, Color.Pink);
        }
        if (history[index] is Measurement)
        {
          if (showColors)
            (history[index] as Measurement).Draw(currentState, shade, "history:measurements");
          else
            (history[index] as Measurement).Draw(currentState, 0.9999, "history:measurements");
          Color color = DrawingTools.GetColorForSensor((history[index] as Measurement).sensor);
          int yIndex = DrawingTools.GetIndexForSensor((history[index] as Measurement).sensor);
          DebugDrawingTool.CreateLine("time_line:measurements", new Vector2(timeLineIndex * 2, yIndex * 10), new Vector2(timeLineIndex * 2 + 2, yIndex * 10), 3, color);
          if (++numberOfObservations >= maxNumberOfObservations) break;
        }
      }

      if (simulatedState is WorldState1D) currentState = new WorldState1D(0);
      else if (simulatedState is WorldState2D) currentState = new WorldState2D(0, 0);
      else if (simulatedState is WorldStatePose2D) currentState = new WorldStatePose2D();
      else currentState = new WorldState2D(0, 0); // to be not uninitialized

      for (index = history.Count - 1; index >= 0; index--)
      {
        shade = 1.0 - (index + 1) / (double)history.Count;
        if (history[index] is ActionDescription)
        {
          currentState = (history[index] as ActionDescription).ExecuteInverted(currentState);
          currentState.DrawShaded(shade, Color.Blue, "history:world states");
        }
      }
    }
    */
    public void Draw(WorldState simulatedState)
    {
      //coordinate system
      DebugDrawingTool.CreateLine("history:_coordinate system", new Vector2(-800, 0), new Vector2(800, 0), 3, Color.Gray);
      DebugDrawingTool.CreateLine("history:_coordinate system", new Vector2(0, -800), new Vector2(0, 800), 3, Color.Gray);

      DebugDrawingTool.CreateLine("time_line:_coordinate system", new Vector2(-800, 0), new Vector2(800, 0), 3, Color.Gray);
      DebugDrawingTool.CreateLine("time_line:_coordinate system", new Vector2(0, -800), new Vector2(0, 800), 3, Color.Gray);

      CalculateRelativePlaces(simulatedState, int.MaxValue);
      List<RelativePlaceWithMeasurement> selectedRelativePlacesWithMeasurement = GetSelectedRelativePlaces(simulatedState);

      foreach (RelativePlaceWithMeasurement entry in selectedRelativePlacesWithMeasurement)
      {
        int yIndex = DrawingTools.GetIndexForSensor(entry.measurement.sensor);
        DebugDrawingTool.CreateLine("time_line:measurements",
          new Vector2(entry.indexInTimeLine * 2, yIndex * 10),
          new Vector2(entry.indexInTimeLine * 2 + 2, yIndex * 10), 7, Color.Black);
      }
      for (int index = relativePlacesWithMeasurement.Count - 1; index >= 0; index--)
      {
        RelativePlaceWithMeasurement entry = relativePlacesWithMeasurement[index];
        double shade = 1.0 - (entry.indexInHistory + 1) / (double)(history.Count + 1);
        entry.measurement.Draw(entry.worldState, shade, "history:measurements");
        Color color = DrawingTools.GetColorForSensor(entry.measurement.sensor);
        int yIndex = DrawingTools.GetIndexForSensor(entry.measurement.sensor);
        DebugDrawingTool.CreateLine("time_line:measurements", 
          new Vector2(entry.indexInTimeLine * 2, yIndex * 10),
          new Vector2(entry.indexInTimeLine * 2 + 2, yIndex * 10), 3, color);
        DebugDrawingTool.CreateCircle("time_line:measurements",
          new Vector2(entry.indexInTimeLine * 2, yIndex * 10), entry.indexInHistory / 10.0 , 1, color);
      }
      int counter = 0;
      foreach (WorldState entry in relativePlaces)
      {
        double shade = (counter++) / (double)(history.Count);
        entry.DrawShaded(shade, Color.Blue, "history:world states");
      }
    }
    public void Save()
    {
      FileInfo file = new FileInfo(DrawingTools.GetPath() + "test.txt");
      StreamWriter streamWriter = file.CreateText();
      foreach (object entry in history)
      {
        if (entry is WorldState)
          streamWriter.WriteLine("state: " + (entry as WorldState).ToString());
        if (entry is ActionDescription)
          streamWriter.WriteLine("action: " + (entry as ActionDescription).ToString());
        if (entry is Measurement)
          streamWriter.WriteLine("measurement: " + (entry as Measurement).ToString());
      }
      streamWriter.Close();
      relativePlacesCalculated = false;
    }

    #region Landmark Selection
    private List<RelativePlaceWithMeasurement> SelectIntelligent(WorldState simulatedState, Experiment experiment, List<RelativePlaceWithMeasurement> list)
    {
      //Sort the list
      int b = 20;
//      int n = 3;
      int[] bestListIdx = new int[3];
      double bestValue = Double.MaxValue;

      if (list.Count >= b)
      {
        List<RelativePlaceWithMeasurement>[] tmpList = new List<RelativePlaceWithMeasurement>[10 + 1];
        tmpList[0] = new List<RelativePlaceWithMeasurement>();
        for (int i = 0; i < b; i++)
        {
          tmpList[0].Add(list[i]);
        }//end for



        for (int i_1 = 0; i_1 < 10; i_1++)
        {
          RelativePlaceWithMeasurement[] placeList = new RelativePlaceWithMeasurement[b - i_1 - 1];
          tmpList[i_1 + 1] = new List<RelativePlaceWithMeasurement>();
          int worseIndex = 0;

          for (int i_2 = 0; i_2 < b - i_1; i_2++) // i_2 wird ausgelassen
          {
            int idx = 0;
            for (int i_3 = 0; i_3 < b - i_1; i_3++)
            {
              if (i_3 != i_2) placeList[idx++] = tmpList[i_1][i_3];
            }//end for
            double value = MBLTools.evaluateLM(simulatedState, placeList, experiment);


            if (i_2 == 0)
            {
              bestValue = value;
              worseIndex = i_2;
              continue;
            }//end if

            if (value < bestValue)
            {
              tmpList[i_1 + 1].Add(tmpList[i_1][worseIndex]);
              bestValue = value;
              worseIndex = i_2;
            }
            else
            {
              tmpList[i_1 + 1].Add(tmpList[i_1][i_2]);
            }//end else
          }//end for
        }//end for

//        for (int i = 0; i < 10; i++)
//          markedAsUsed[tmpList[10][i].indexInHistory] = true;

        return tmpList[10];
      }//end if



      /*
      if (list.Count >= b && false)
      {
        for (int i_1 = 0; i_1 < b - 4; i_1++)
          for (int i_2 = i_1 + 1; i_2 < b - 3; i_2++)
            for (int i_3 = i_2 + 1; i_3 < b - 2; i_3++)
              for (int i_4 = i_3 + 1; i_4 < b - 1; i_4++)
                for (int i_5 = i_4 + 1; i_5 < b; i_5++)
                {
                  RelativePlacesInfo[] placeList = new RelativePlacesInfo[5] { list[i_1], list[i_2], list[i_3], list[i_4], list[i_5] };
                  double value = MBLTools.evaluateLM(simulatedState, placeList, experiment);
                  if (value > bestValue)
                  {
                    bestListIdx = new int[] { i_1, i_2, i_3, i_4, i_5 };
                    bestValue = value;
                  }//end if
                }//end for
        List<RelativePlacesInfo> bestList = new List<RelativePlacesInfo>();
        for (int i = 0; i < b; i++)
        {
          if (!contains(i, bestListIdx))
          {
            bestList.Add(list[i]);
//            markedAsUsed[list[i].indexInHistory] = true;
          }//end if
        }//end for
        return bestList;
      }//end if
      */


      return list;
    }
    private bool contains(int idx, int[] array)
    {
      for (int i = 0; i < array.Length; i++)
        if (idx == array[i])
          return true;
      return false;
    }
    private List<RelativePlaceWithMeasurement> SelectLastN(WorldState simulatedState, List<RelativePlaceWithMeasurement> list)
    {
      List<RelativePlaceWithMeasurement> toReturn = new List<RelativePlaceWithMeasurement>();
      int counter = 0;
      foreach (RelativePlaceWithMeasurement item in list)
      {
        toReturn.Add(item);
        if (counter++ > Globals.selfLocatorProperties._ParamLastN) return toReturn;
      }
      return toReturn;
    }
    private List<RelativePlaceWithMeasurement> SelectLastNOfEach(WorldState simulatedState, List<RelativePlaceWithMeasurement> list)
    {
      int counter = 0;
      List<RelativePlaceWithMeasurement> toReturn = new List<RelativePlaceWithMeasurement>();
      Dictionary<string, int> listOfIds = new Dictionary<string, int>();
      foreach (RelativePlaceWithMeasurement item in list)
      {
        string currentID = ""; 
        if (item.measurement.sensor is BearingSensor) currentID = (item.measurement.sensor as BearingSensor).landmarkID.ToString();
        if (item.measurement.sensor is BearingSensorAmbiguous) currentID = (item.measurement.sensor as BearingSensorAmbiguous).landmarkClass.ToString();
        if (!listOfIds.ContainsKey(currentID))
        {
          listOfIds[currentID] = 0;
        }
        if (listOfIds[currentID] < Globals.selfLocatorProperties._NOfSubsequentObservations)
        {
          toReturn.Add(item);
          listOfIds[currentID]++;
        }
        if (counter++ > Globals.selfLocatorProperties._ParamLastN) return toReturn;
      }
      return toReturn;
    }
    private List<RelativePlaceWithMeasurement> SelectLastAndFirstOfEachInLastN(WorldState simulatedState, List<RelativePlaceWithMeasurement> list)
    {
      int counter = 0;
      List<RelativePlaceWithMeasurement> toReturn = new List<RelativePlaceWithMeasurement>();
      Dictionary<string, int> lastOccurenceInTimeLine = new Dictionary<string, int>();
      Dictionary<string, int> firstOccurenceInTimeLine = new Dictionary<string, int>();
      Dictionary<string, RelativePlaceWithMeasurement> bufferedItem = new Dictionary<string, RelativePlaceWithMeasurement>();
      int lastIndexInTimeLine = 0;
      foreach (RelativePlaceWithMeasurement item in list)
      {
        string currentID = "";
        if (item.measurement.sensor is BearingSensor) currentID = (item.measurement.sensor as BearingSensor).landmarkID.ToString();
        if (item.measurement.sensor is BearingSensorAmbiguous) currentID = (item.measurement.sensor as BearingSensorAmbiguous).landmarkClass.ToString();
        if (!lastOccurenceInTimeLine.ContainsKey(currentID))
        {
          toReturn.Add(item);
          lastOccurenceInTimeLine[currentID] = item.indexInTimeLine;
          firstOccurenceInTimeLine[currentID] = item.indexInTimeLine;
        }
        if (item.indexInTimeLine > lastOccurenceInTimeLine[currentID])
        {
          lastOccurenceInTimeLine[currentID] = item.indexInTimeLine;
          bufferedItem[currentID] = item;
        }

        if (lastIndexInTimeLine != item.indexInTimeLine)
        {
          List<string> keys = new List<string>();
          foreach (string key in lastOccurenceInTimeLine.Keys) keys.Add(key);
          foreach (string id in keys)
          {
            if (
              item.indexInTimeLine > lastOccurenceInTimeLine[id] + Globals.selfLocatorProperties._MaxGapForBlocks &&
              lastOccurenceInTimeLine[id] - firstOccurenceInTimeLine[id] > Globals.selfLocatorProperties._MinSizeForBlocks)
            {
              lastOccurenceInTimeLine[id] = int.MaxValue;
              if (bufferedItem.ContainsKey(id)) toReturn.Add(bufferedItem[id]);
            }
          }
        }
        lastIndexInTimeLine = item.indexInTimeLine;
        if (counter++ > Globals.selfLocatorProperties._ParamLastN) return toReturn;
        if (toReturn.Count > Globals.selfLocatorProperties._MaxNumberOfObservations)
        {
          toReturn.RemoveRange(
            Globals.selfLocatorProperties._MaxNumberOfObservations, 
            toReturn.Count - Globals.selfLocatorProperties._MaxNumberOfObservations);
          return toReturn;
        }
      }
      List<string> keys2 = new List<string>();
      foreach (string key in lastOccurenceInTimeLine.Keys) keys2.Add(key);
      foreach (string id in keys2)
      {
        if (bufferedItem.ContainsKey(id)) toReturn.Add(bufferedItem[id]);
      }
      if (toReturn.Count > Globals.selfLocatorProperties._MaxNumberOfObservations)
      {
        toReturn.RemoveRange(
          Globals.selfLocatorProperties._MaxNumberOfObservations,
          toReturn.Count - Globals.selfLocatorProperties._MaxNumberOfObservations);
        return toReturn;
      }
      return toReturn;
    }
    private List<RelativePlaceWithMeasurement> SelectLastAndFirstOfEachInLastN2(WorldState simulatedState, List<RelativePlaceWithMeasurement> list)
    {
      List<RelativePlaceWithMeasurement> toReturn = new List<RelativePlaceWithMeasurement>();
      Dictionary<string, int> lastOccurenceInTimeLine = new Dictionary<string, int>();
      Dictionary<string, int> firstOccurenceInTimeLine = new Dictionary<string, int>();
      Dictionary<string, List<RelativePlaceWithMeasurement>> bufferedItems = new Dictionary<string, List<RelativePlaceWithMeasurement>>();
      Dictionary<string, int> lastBufferedOccurenceInTimeLine = new Dictionary<string, int>();
      System.Collections.SortedList sortedList = new System.Collections.SortedList();
      System.Collections.Generic.SortedDictionary<double, string> sd = new SortedDictionary<double, string>();

      int lastIndexInTimeLine = 0;
      foreach (RelativePlaceWithMeasurement item in list)
      {
        if (lastIndexInTimeLine > Globals.selfLocatorProperties._ParamLastN) break;
        string currentID = "";
        if (item.measurement.sensor is BearingSensor) currentID = (item.measurement.sensor as BearingSensor).landmarkID.ToString();
        if (item.measurement.sensor is BearingSensorAmbiguous) currentID = (item.measurement.sensor as BearingSensorAmbiguous).landmarkClass.ToString();
        if (!lastOccurenceInTimeLine.ContainsKey(currentID))
        {
          bufferedItems[currentID] = new List<RelativePlaceWithMeasurement>();
          lastOccurenceInTimeLine[currentID] = item.indexInTimeLine;
          firstOccurenceInTimeLine[currentID] = item.indexInTimeLine;
          bufferedItems[currentID].Add(item);
          lastBufferedOccurenceInTimeLine[currentID] = item.indexInTimeLine;
        }

//        if (lastOccurenceInTimeLine[currentID] - firstOccurenceInTimeLine[currentID] < Globals.selfLocatorProperties._MinSizeForBlocks)
//        {
//          MarkEntryInTimeLine(item.indexInTimeLine, DrawingTools.GetIndexForSensor(item.measurement.sensor), Color.Green, 12);
//        }
//        else 
        if (item.indexInTimeLine > Math.Max(lastOccurenceInTimeLine[currentID] + Globals.selfLocatorProperties._MaxGapForBlocks, lastOccurenceInTimeLine[currentID]) )
        {
          lastOccurenceInTimeLine[currentID] = int.MaxValue;
          MarkEntryInTimeLine(item.indexInTimeLine, DrawingTools.GetIndexForSensor(item.measurement.sensor), Color.Blue, 12);
        }
        
        if (item.indexInTimeLine > lastOccurenceInTimeLine[currentID])
        {
          lastOccurenceInTimeLine[currentID] = item.indexInTimeLine;
          bufferedItems[currentID].Add(item);
          lastBufferedOccurenceInTimeLine[currentID] = item.indexInTimeLine;
          MarkEntryInTimeLine(item.indexInTimeLine, DrawingTools.GetIndexForSensor(item.measurement.sensor), Color.Orange, 10);
        }
        lastIndexInTimeLine = item.indexInTimeLine;
      }

      List<string> keys = new List<string>();
      foreach (string key in lastOccurenceInTimeLine.Keys)
      {
        keys.Add(key);
        double index = firstOccurenceInTimeLine[key];
        while (sd.ContainsKey(index)) index += 0.01;
        sd[index] = key;

        if (lastBufferedOccurenceInTimeLine[key] - firstOccurenceInTimeLine[key] > Globals.selfLocatorProperties._MinSizeForBlocks)
        {
          index = lastBufferedOccurenceInTimeLine[key];
          while (sd.ContainsKey(index)) index += 0.01;
          sd[index] = key;
        }
      }

      int counter = 0;
      double maxIndex = 0;
      foreach (double index2 in sd.Keys)
      {
        MarkEntryInTimeLine(index2, 0, Color.Orange, 10);
        if (counter++ < Globals.selfLocatorProperties._MaxNumberOfObservations) maxIndex = index2;
      }

      MarkEntryInTimeLine(maxIndex, 0, Color.Red, 10);

      int c = 0;
      List<string> keys2 = new List<string>();
      foreach (string key in lastOccurenceInTimeLine.Keys) keys2.Add(key);
      foreach (string id in keys2)
      {
        if (bufferedItems.ContainsKey(id) && c < Globals.selfLocatorProperties._ParamLastN)
        {
          int d = 0;
          foreach (RelativePlaceWithMeasurement item in bufferedItems[id])
          {
            bool firstOccurenceIsOK = firstOccurenceInTimeLine[id] <= maxIndex;
            bool lastOccurenceIsOK = lastBufferedOccurenceInTimeLine[id] <= maxIndex;
            //            if(d == 0 || d == bufferedItems[id].Count - 1)
            if (firstOccurenceIsOK && d < Globals.selfLocatorProperties._NOfSubsequentObservations ||
              lastOccurenceIsOK && d >= bufferedItems[id].Count - Globals.selfLocatorProperties._NOfSubsequentObservations)
            toReturn.Add(item);
            d++;
          }
          c++;
        }
      }
      /*
      if (toReturn.Count > Globals.selfLocatorProperties._MaxNumberOfObservations)
      {
        toReturn.RemoveRange(
          Globals.selfLocatorProperties._MaxNumberOfObservations,
          toReturn.Count - Globals.selfLocatorProperties._MaxNumberOfObservations);
        return toReturn;
      }*/
      return toReturn;
    }

    private static void MarkEntryInTimeLine(double xPosition, int yIndex, Color color, int width)
    {
      DebugDrawingTool.CreateLine("time_line:landmark selection",
        new Vector2(xPosition * 2.0, yIndex * 10),
        new Vector2(xPosition * 2.0 + 2, yIndex * 10), width, color);
    }
    #endregion

    public struct RelativePlaceWithMeasurement
    {
      public RelativePlaceWithMeasurement(WorldState worldState, Measurement measurement, int indexInHistory, int indexInTimeLine)
      {
        this.worldState = worldState;
        this.measurement = measurement;
        this.indexInHistory = indexInHistory;
        this.indexInTimeLine = indexInTimeLine;
      }
      public WorldState worldState;
      public Measurement measurement;
      public int indexInHistory;
      public int indexInTimeLine;
    }

  }
}
